#ifndef BTN7919_H
#define BTN7919_H
/*
  Motor <- BTN7919 <- ESP32 
           front
          /------\
  left   |        | right 
           rear

 左电机 + 黑线 - 电机1 out1 线圈侧 - BTN7919 PMW1 - GPIO 32 黄色
 左电机 - 红线 - 电机1 out1 开关侧 - BTN7919 PMW2 - GPIO 33 绿色

 右电机 + 红线 - 电机2 out2 线圈侧 - BTN7919 PMW3 - GPIO 26 黄色
 右电机 - 黑线 - 电机2 out2 开关侧 - BTN7919 PMW4 - GPIO 27 绿色 
*/

// Motor left
int motor_left_Pin_P = 32;  // 黄色 PMW1 motor black line
int motor_left_Pin_N = 33;  //  绿色 PMW2 motor red line

// Motor right
int motor_right_Pin_P = 26;  // 黄色  PMW3  motor red line
int motor_right_Pin_N = 27;  // 绿色 PMW4   motor black line

// robot action code
const int DEV_GO = 1;
const int DEV_BACK = 2;
const int DEV_LEFT = 3;
const int DEV_RIGHT = 4;
const int DEV_STOP = 5;
//
const int DEV_SPEED_UP = 6;
const int DEV_SPEED_DOWN = 7;
const int DEV_LEFT_SPEED_UP = 13;
const int DEV_LEFT_SPEED_DOWN = 11;
const int DEV_RIGHT_SPEED_UP = 14;
const int DEV_RIGHT_SPEED_DOWN = 12;

// car running state
int car_state = DEV_STOP;

// PWM - speed
int left_speed = 200;
int right_speed =  left_speed+10;
const int speed_step = 10;

void car_forward() {
  analogWrite(motor_left_Pin_P, left_speed);
  digitalWrite(motor_left_Pin_N, LOW);

  analogWrite(motor_right_Pin_P, right_speed);
  digitalWrite(motor_right_Pin_N, LOW);
}

void car_back() {
  digitalWrite(motor_left_Pin_P, LOW);
  analogWrite(motor_left_Pin_N, left_speed);
  digitalWrite(motor_right_Pin_P, LOW);
  analogWrite(motor_right_Pin_N, right_speed);
}

void car_stop() {
  // Stop the DC motor
  digitalWrite(motor_right_Pin_P, LOW);
  digitalWrite(motor_right_Pin_N, LOW);

  digitalWrite(motor_left_Pin_P, LOW);
  digitalWrite(motor_left_Pin_N, LOW);
}

void car_turn_left() {
  // stop left motor
  digitalWrite(motor_left_Pin_P, LOW);
  digitalWrite(motor_left_Pin_N, LOW);
  // rotor right motor
  analogWrite(motor_right_Pin_P, right_speed);
  digitalWrite(motor_right_Pin_N, LOW);
}

void car_turn_right() {
  // rotor left motor
  analogWrite(motor_left_Pin_P, left_speed);
  digitalWrite(motor_left_Pin_N, LOW);
  // stop  right motor
  digitalWrite(motor_right_Pin_P, LOW);
  digitalWrite(motor_right_Pin_N, LOW);
 }

void running_with_new_speed() {
  switch (car_state) {
    case DEV_GO:
      car_forward();
      break;
    case DEV_BACK:
      car_back();
      break;
    case DEV_LEFT:
      car_turn_left();
      break;
    case DEV_RIGHT:
      car_turn_right();
      break;
    default:
      break;
  }
}

void left_speed_up_value() {
  left_speed += speed_step;
  if (left_speed > 255) {
    left_speed = 255;
  }
}


void left_speed_down_value() {
  left_speed -= speed_step;
  if (left_speed < 0) {
    left_speed = 0;
  }
}

void right_speed_up_value() {
  right_speed += speed_step;
  if (right_speed > 255) {
    right_speed = 255;
  }
}

void right_speed_down_value() {
  right_speed -= speed_step;
  if (right_speed < 0) {
    right_speed = 0;
  }
}

void car_left_speed_up() {
  left_speed_up_value();
  running_with_new_speed();
}
void car_left_speed_down() {
  left_speed_down_value();
  running_with_new_speed();
}

void car_right_speed_up() {
  right_speed_up_value();
  running_with_new_speed();
}

void car_right_speed_down() {
  right_speed_down_value();
  running_with_new_speed();
}

void car_speed_up() {
  left_speed_up_value();
  right_speed_up_value();
  running_with_new_speed();
}

void car_speed_down() {
  left_speed_down_value();
  right_speed_down_value();
  running_with_new_speed();
}

void car_action(int car_cmd) {
  switch (car_cmd) {
    case DEV_GO:
      car_forward();
      car_state = DEV_GO;
      break;
    case DEV_LEFT:
      car_turn_left();
      car_state = DEV_LEFT;
      //delay(100);
      //car_stop();
      //car_state = DEV_STOP;
      break;
    case DEV_RIGHT:
      car_turn_right();
      car_state = DEV_RIGHT;
      //  delay(100);
      //  car_stop();
      //  car_state = DEV_STOP;
      break;
    case DEV_BACK:
      car_back();
      car_state = DEV_BACK;
      break;
    case DEV_STOP:
      car_stop();
      car_state = DEV_STOP;
      break;
    case DEV_SPEED_UP:
      car_speed_up();
      break;
    case DEV_SPEED_DOWN:
      car_speed_down();
      break;
    case DEV_LEFT_SPEED_UP:
      car_left_speed_up();
      break;
    case DEV_LEFT_SPEED_DOWN:
      car_left_speed_down();
      break;  
    case DEV_RIGHT_SPEED_UP:
      car_right_speed_up();
      break;
    case DEV_RIGHT_SPEED_DOWN:
      car_right_speed_down();
      break;    
    default:
      break;
  }  // switch
};

void setup_btn7919() {
  // sets the pins as outputs:
  pinMode(motor_right_Pin_P, OUTPUT);
  pinMode(motor_right_Pin_N, OUTPUT);

  pinMode(motor_left_Pin_P, OUTPUT);
  pinMode(motor_left_Pin_N, OUTPUT);
}


void loop_btn7919() {
}

#endif /* BTN7919_H */